/*
 * @Author: your name
 * @Date: 2021-06-12 07:28:47
 * @LastEditTime: 2021-06-22 00:08:54
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: /lab_1_2_geometry_unfinished/perspective_camera_model.cpp
 */
#include "perspective_camera_model.h"
#include "opencv2/core/eigen.hpp"
#include "opencv2/imgproc.hpp"

PerspectiveCameraModel::PerspectiveCameraModel(const Eigen::Matrix3d& K,
                                               const Sophus::SE3d& pose_world_camera,
                                               const Vector5d& distortion_coeffs)
  : K_{K}
  , pose_world_camera_{pose_world_camera}
  , distortion_coeffs_{distortion_coeffs}
{
  camera_projection_matrix_ = computeCameraProjectionMatrix();
}

Sophus::SE3d PerspectiveCameraModel::getPose() const
{
  return pose_world_camera_;
}

Eigen::Matrix3d PerspectiveCameraModel::getCalibrationMatrix() const
{
    // std::cout<<K_<<std::endl;
  return K_;
}

PerspectiveCameraModel::Matrix34d PerspectiveCameraModel::getCameraProjectionMatrix() const
{
  return camera_projection_matrix_;
}

Eigen::Vector2d PerspectiveCameraModel::projectWorldPoint(Eigen::Vector3d world_point) const
{
  // Todo: Implement projection using camera_projection_matrix_.
  Eigen::Vector4d Pw=world_point.homogeneous();
  
  Eigen::Matrix<double, 3, 4> KTcw=this->computeCameraProjectionMatrix();
  Eigen::Vector2d Pc=(KTcw*Pw).hnormalized();
  Eigen::Vector3d Pcprem=KTcw*Pw;
  int pixx=static_cast<int>( (Pcprem(0)/Pcprem(2)));
  int pixy=static_cast<int>( (Pcprem(1)/Pcprem(2)));
  
  // if(pixy>0&&pixy<936&&pixx<1404&&pixx>0)
  std::cout <<"standard Pc"<<Pc<<std::endl;
  std::cout<<"myPc"<<Pcprem<<std::endl;
  std::cout<<"("<<pixx<<","<<pixy<<")"<<std::endl;
  std::cout<<"standard :"<<"("<<Pc.x()<<","<<Pc.y()<<")"<<std::endl;
  
  return Eigen::Vector2d(pixx,pixy); 
}

Eigen::Matrix2Xd PerspectiveCameraModel::projectWorldPoints(Eigen::Matrix3Xd /*world_points*/) const
{
  // Todo: Optionally implement projection using camera_projection_matrix_.
  return Eigen::Matrix2Xd();
}

PerspectiveCameraModel::Matrix34d PerspectiveCameraModel::computeCameraProjectionMatrix() const
{
//   Matrix34d P = Matrix34d::Zero();
//   P.leftCols<3>() = Eigen::Matrix3d::Identity(); 

  
  return   K_*pose_world_camera_.inverse().matrix3x4();
}

cv::Mat PerspectiveCameraModel::undistortImage(cv::Mat distorted_image) const
{
  // Convert to cv::Mats
  cv::Mat K_cv;
  cv::eigen2cv(K_, K_cv);
  cv::Mat dist_coeffs_cv;
  cv::eigen2cv(distortion_coeffs_, dist_coeffs_cv);

  // Undistort image.
  cv::Mat undistorted_image;
//   cv::imshow("distorted_image",distorted_image);
  cv::undistort(distorted_image, undistorted_image, K_cv, dist_coeffs_cv);
  return undistorted_image;
    // return distorted_image;
}
